#ifndef MATRIX_44_H
#define MATRIX_44_H

#include "vect.h"
#include <string>
#include <iomanip>
#include <sstream>
using namespace std;

/*!
	Enkapsuluje macierz 4x4. Jest to macierz w rozumieniu OpenGL'a. Czyli macierz column-order, zorganizowana w taki sposob:

	M = |m1 m5 m9  m13|
		|m2 m6 m10 m14|
		|m3 m7 m11 m15|
		|m4 m8 m12 m16|
*/
template< class T >
class Matrix44
{
 private:	

	T m[ 16 ];

 public:

	//konsturktor
	Matrix44()
	{ }

	//! konstruktor
	/*!
		Konstruktor dostaje wskaznik do typu T. Wskaznik ten traktowany jest jak tablica 15 nasto elementowa gdzie kolejno na elemantach
		c[0] .. c[15] znajduja sie kolejne wartosci macierzy.
	*/
	Matrix44( const T* c )
	{
		for( int ii = 0; ii < 16; ++ii )
			m[ ii ] = c[ ii ];
	}

	//konstruktor kopiujacy
	Matrix44( const Matrix44< T >& M );

	void operator=( const Matrix44< T >& M );

	Matrix44< T > operator*( const T& scalar );

	//! Operator mnozenia macierzy przez punkt/wektor. Jesli p chcemy traktowac jako punkt (wektor) nalezy czwarta wspolrzedna (ktora nie jest reprezentowana w klasie) traktowac jako 1 (0)
	Vect<T> operator*( const Vect<T>& p );

	T& operator[]( const int ith )
	{
		assert( ith >= 0 && ith <= 15 && "Matrix (Matrix44) index out of range" );
		return m[ ith ];
	}

	void LoadIdentity();
	void LoadRotationAboutArbitrary( float angle, T x, T y, T z );

	void SetMatrixColumn( int cNr, T x, T y, T z, T w );

	const T* GetDataPointer() const
	{ return &(m[0]); }

	//! Wymnaza ta macierz przez macierz skali (patrz nizej).
	/*!
		Macierz skali: |s1 0 0 0|
					   |0 s2 0 0|
					   |0 0 s3 0|
					   |0 0 0 s4|
	*/
	void MultiplyByScaleMatrix( T s1, T s2, T s3, T s4 );

	//! Zwraca macierz w postaci stringa. w okresla na jakiej szerokosci wypisana bedzie kazda liczba w macierzy
	string ToString( int w );

	//brak definicji dla tych funkcji!
	void LoadRotationAboutX( float angle );
	void LoadRotationAboutY( float angle );
	void LoadRotationAboutZ( float angle );
};
//------------------------------------------- Uwaga ------------------------------------------------------//
/*
	Klasa szablonowa, wszsytko musi isc do jednego pliku (?)
*/
//------------------------------------------- ToString ---------------------------------------------------//
template< class T >
string Matrix44< T >::ToString( int w )
{
	//M = |m1 m5 m9  m13|
	//	  |m2 m6 m10 m14|
	//	  |m3 m7 m11 m15|
	//	  |m4 m8 m12 m16|

	stringstream s;
	s  << "|" << setw(w) << m[0] << setw(w) << m[4] << setw(w) << m[8] << setw(w) << m[12] << "|" << endl;
	s  << "|" << setw(w) << m[1] << setw(w) << m[5] << setw(w) << m[9] << setw(w) << m[13] << "|" << endl;
	s  << "|" << setw(w) << m[2] << setw(w) << m[6] << setw(w) << m[10] << setw(w) << m[14] << "|" << endl;
	s  << "|" << setw(w) << m[3] << setw(w) << m[7] << setw(w) << m[11] << setw(w) << m[15] << "|";

	return s.str();
}
//------------------------------------------- MultiplyUpper3x3ByScaleMatrix ------------------------------//
template< class T >
void Matrix44< T >::MultiplyByScaleMatrix( T s1, T s2, T s3, T s4 )
{
	SetMatrixColumn( 1, m[0]*s1, m[1]*s1, m[2]*s1, m[3]*s1 );
	SetMatrixColumn( 2, m[4]*s2, m[5]*s2, m[6]*s2, m[7]*s2 );
	SetMatrixColumn( 3, m[8]*s3, m[9]*s3, m[10]*s3, m[11]*s3 );
	SetMatrixColumn( 4, m[12]*s4, m[13]*s4, m[14]*s4, m[15]*s4 );
}
//------------------------------------------- SetMatrixColumn --------------------------------------------//
template< class T >
void Matrix44< T >::SetMatrixColumn(int cNr, T x, T y, T z, T w)
{
	--cNr;

	m[ cNr*4 + 0 ] = x;
	m[ cNr*4 + 1 ] = y;
	m[ cNr*4 + 2 ] = z;
	m[ cNr*4 + 3 ] = w;
}
//------------------------------------------- LoadIdentity -----------------------------------------------//
template< class T >
void Matrix44< T >::LoadIdentity()
{
	m[ 0 ] = 1;
	m[ 1 ] = 0;
	m[ 2 ] = 0;
	m[ 3 ] = 0;

	m[ 4 ] = 0;
	m[ 5 ] = 1;
	m[ 6 ] = 0;
	m[ 7 ] = 0;
	
	m[ 8 ] = 0;
	m[ 9 ] = 0;
	m[ 10 ] = 1;
	m[ 11 ] = 0;

	m[ 12 ] = 0;
	m[ 13 ] = 0;
	m[ 14 ] = 0;
	m[ 15 ] = 1;
}

//------------------------------------------- LoadRotationAboutArbitrary ----------------------------------//
template< class T >
void Matrix44< T >::LoadRotationAboutArbitrary( float angle, T x, T y, T z )
{
	T mag, s, c;
	T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;

	s = T(sin(angle));
	c = T(cos(angle));

	mag = T(sqrt( x*x + y*y + z*z ));

	// Identity matrix
	if (mag == 0.0f) 
	{
		LoadIdentity();
		return;
	}

	// Rotation matrix is normalized
	x /= mag;
	y /= mag;
	z /= mag;

	#define M(row,col)  m[col*4+row]

		xx = x * x;
		yy = y * y;
		zz = z * z;
		xy = x * y;
		yz = y * z;
		zx = z * x;
		xs = x * s;
		ys = y * s;
		zs = z * s;
		one_c = 1.0f - c;

		M(0,0) = (one_c * xx) + c;
		M(0,1) = (one_c * xy) - zs;
		M(0,2) = (one_c * zx) + ys;
		M(0,3) = 0.0f;

		M(1,0) = (one_c * xy) + zs;
		M(1,1) = (one_c * yy) + c;
		M(1,2) = (one_c * yz) - xs;
		M(1,3) = 0.0f;

		M(2,0) = (one_c * zx) - ys;
		M(2,1) = (one_c * yz) + xs;
		M(2,2) = (one_c * zz) + c;
		M(2,3) = 0.0f;

		M(3,0) = 0.0f;
		M(3,1) = 0.0f;
		M(3,2) = 0.0f;
		M(3,3) = 1.0f;

	#undef M
}

//---------------------------------------------- copy constructor -----------------------------------------///
template< class T >
Matrix44< T >::Matrix44( const Matrix44< T >& M )
{
	for( int ii = 0; ii < 16; ++ii )
		m[ ii ] = M.m[ ii ];
}
//---------------------------------------------- operator= ------------------------------------------------//
template< class T >
void Matrix44< T >::operator=( const Matrix44< T >& M )
{
	for( int ii = 0; ii < 16; ++ii )
		m[ ii ] = M.m[ ii ];
}
//------------------------------------------------ operator* ----------------------------------------------//
template< class T >
Matrix44< T > Matrix44< T >::operator*( const T& scalar )
{
	Matrix44< T > M;

	for( int ii = 0; ii < 16; ++ii )
		M.m[ ii ] = m[ ii ] * scalar;

	return M;
}
//----------------------------------------------- operator* ---------------------------------------------//
template< class T >
Vect< T > Matrix44< T >::operator*( const Vect< T >& p )
{
	T x = p.get(1);
	T y = p.get(2);
	T z = p.get(3);

	Vect< T > r;
	r.set( 1, m[0]*x + m[4]*y + m[8]*z + m[12] );
	r.set( 2, m[1]*x + m[5]*y + m[9]*z + m[13] );
	r.set( 3, m[2]*x + m[6]*y + m[10]*z + m[14] );

	return r;
}


#endif